/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package metodogauss;

import java.awt.Point;
import java.util.ArrayList;
import java.util.Random;
import javax.vecmath.Point3d;

/**
 *
 * @author Igor
 */
public class MetodoGauss {


    private ArrayList<Point3d> pontosMundo = new ArrayList<Point3d>();
    private ArrayList<Point> pontosCamera = new ArrayList<Point>();
    private double[][] Matriz = new double [12][13];
    private double[] arrayAuxLinha = new double[13];
    private double[] arrayAuxColuna = new double[12];
    private int[] solucoes = new int[12];
    /**
     * @param args the command line arguments
     */

    public static void main(String[] args) {
        MetodoGauss gauss = new MetodoGauss();
        gauss.pontos();
        gauss.montaMatriz();
        gauss.imprimeMatriz();
        gauss.solveByGauss();
        System.out.println("\n\n-------------------------------------\n\n");
        gauss.zeraValores();
        gauss.imprimeMatriz();
        gauss.resolveSistema();
        
    }

    public  void montaMatriz() {
        int j;
        for (int i=0;i<6;i++) {
            j = 2*i;
            Matriz[j][0] =  (int) (pontosMundo.get(i).x); //a11
            Matriz[j][1] =  (int) (pontosMundo.get(i).y); //a12
            Matriz[j][2] =  (int) (pontosMundo.get(i).z); //a13
            Matriz[j][3] =  (1); //a14
            Matriz[j][4] = 0; //a21
            Matriz[j][5] = 0; //a22
            Matriz[j][6] = 0; //a23
            Matriz[j][7] = 0; //a24
            Matriz[j][8] =  (int) (pontosCamera.get(i).getX()*pontosMundo.get(i).x); //a41
            Matriz[j][9] =  (int) (-1*pontosCamera.get(i).getY()*pontosMundo.get(i).x); //a42
            Matriz[j][10] =  (int) (-1*pontosCamera.get(i).getX()*pontosMundo.get(i).z); //a43
            Matriz[j][11] =  (int) (-1*pontosCamera.get(i).getX()); //a44
            Matriz[j][12] = 0;

            j++;
            Matriz[j][0] =  0; //a11
            Matriz[j][1] =  0; //a12
            Matriz[j][2] =  0; //a13
            Matriz[j][3] =  0; //a14
            Matriz[j][4] = pontosMundo.get(i).x; //a21
            Matriz[j][5] = pontosMundo.get(i).y; //a22
            Matriz[j][6] = pontosMundo.get(i).z; //a23
            Matriz[j][7] = 1; //a24
            Matriz[j][8] =  (int) (-1*pontosCamera.get(i).getX()*pontosMundo.get(i).x); //a41
            Matriz[j][9] =  (int) (-1*pontosCamera.get(i).getY()*pontosMundo.get(i).y); //a42
            Matriz[j][10] =  (int) (-1*pontosCamera.get(i).getY()*pontosMundo.get(i).z); //a43
            Matriz[j][11] =  (int) (-1*pontosCamera.get(i).getY()); //a44
            Matriz[j][12] = 0;
        }
    }

    public void solveByGauss() {

        double pivo=1;

        for (int i=0;i<12;i++) {
            for (int j=0;j<13;j++) {
                if (i==j) {
                    if (Matriz[i][j]==0) {
                        trocaLinhaouColuna(i,j);
                    }
                    pivo = Matriz[i][j];
                    zeraColunas(pivo,i,j);
                }
            }
        }

        resolveSistema();
    }

    public void pontos() {
        Random rand = new Random();
        for (int i=1;i<7;i++) {
            pontosCamera.add(new Point (rand.nextInt(10),rand.nextInt(15)));
            pontosMundo.add(new Point3d (rand.nextInt(19),rand.nextInt(22),rand.nextInt(6)));
        }
        
    }

    private void trocaLinhaouColuna(int i,int j) {
          arrayAuxLinha = Matriz[i];
          boolean inverteu = false;

          for (int k=i+1;k<12;k++) {
              if (Matriz[k][j]!=0) {
                  Matriz[i] = Matriz[k];
                  Matriz[k] = arrayAuxLinha;
                  inverteu = true;
                  break;
              }
          }

          if (inverteu==false) {
              int k=i;
              for (int y=j+1;j<12;j++) {
                    if (Matriz[k][y]!=0) {
                        for (int z=0;z<13;z++) {
                            arrayAuxColuna[z] = Matriz[i][z];
                            Matriz[i][z] = Matriz[k][z];
                            Matriz[k][z] = arrayAuxColuna[z];
                        }

                        inverteu = true;
                        break;
                    }
              }
              
          }

          if (!inverteu)
              System.out.println("sistema impossível do caralho");
    }

    public void imprimeMatriz() {
        for (int i=0;i<12;i++) {
            for (int j=0;j<12;j++) {
                System.out.print(Matriz[i][j] +"  ");
                if (j==11)
                    System.out.println("=  "+ Matriz[i][j+1] +"\n");
            }
        }
    }

    public void zeraColunas(double pivo,int i,int z) {
        double first=0,count;
        for (int k=i+1;k<12;k++) {
            count = 0;
            for (int j=0;j<13;j++) {
                if (count==0) {
                    first = Matriz[k][z];
                    count = 1;
                }
                double b = (double)((first/pivo)*-1)*Matriz[i][j];
                Matriz[k][j] = (double) (b + Matriz[k][j]);
            }
        }

    }

    public void resolveSistema() {
        solucoes[11] = (int) ((Matriz[11][12] / Matriz[11][11]));
        solucoes[10] = (int) (((-1*(Matriz[10][11]*solucoes[11]))+Matriz[10][12])/Matriz[10][10]);
        solucoes[9] = (int) (((-1*((Matriz[9][11]*solucoes[11])+(Matriz[9][10]*solucoes[10])))+Matriz[9][12])/Matriz[9][9]);
        solucoes[8] = (int) (((-1*((Matriz[8][11]*solucoes[11])+(Matriz[8][10]*solucoes[10])+(Matriz[8][9]*solucoes[9])))+Matriz[8][12])/Matriz[8][8]);
        solucoes[7] = (int) (((-1*((Matriz[7][11]*solucoes[11])+(Matriz[7][10]*solucoes[10])+(Matriz[7][9]*solucoes[9])+(Matriz[7][8]*solucoes[8])))+Matriz[7][12])/Matriz[7][7]);
        solucoes[6] = (int) (((-1*((Matriz[6][11]*solucoes[11])+(Matriz[6][10]*solucoes[10])+(Matriz[6][9]*solucoes[9])+(Matriz[6][8]*solucoes[8])+(Matriz[6][7]*solucoes[7])))+Matriz[6][12])/Matriz[6][6]);
        solucoes[5] = (int) (((-1*((Matriz[5][11]*solucoes[11])+(Matriz[5][10]*solucoes[10])+(Matriz[5][9]*solucoes[9])+(Matriz[5][8]*solucoes[8])+(Matriz[5][7]*solucoes[7])+(Matriz[5][6]*solucoes[6])))+Matriz[5][12])/Matriz[5][5]);
        solucoes[4] = (int) (((-1*((Matriz[4][11]*solucoes[11])+(Matriz[4][10]*solucoes[10])+(Matriz[4][9]*solucoes[9])+(Matriz[4][8]*solucoes[8])+(Matriz[4][7]*solucoes[7])+(Matriz[4][6]*solucoes[6])+(Matriz[4][5]*solucoes[5])))+Matriz[4][12])/Matriz[4][4]);
        solucoes[3] = (int) (((-1*((Matriz[3][11]*solucoes[11])+(Matriz[3][10]*solucoes[10])+(Matriz[3][9]*solucoes[9])+(Matriz[3][8]*solucoes[8])+(Matriz[3][7]*solucoes[7])+(Matriz[3][6]*solucoes[6])+(Matriz[3][5]*solucoes[5])+(Matriz[3][4]*solucoes[4])))+Matriz[3][12])/Matriz[3][3]);
        solucoes[2] = (int) (((-1*((Matriz[2][11]*solucoes[11])+(Matriz[2][10]*solucoes[10])+(Matriz[2][9]*solucoes[9])+(Matriz[2][8]*solucoes[8])+(Matriz[2][7]*solucoes[7])+(Matriz[2][6]*solucoes[6])+(Matriz[2][5]*solucoes[5])+(Matriz[2][4]*solucoes[4])+(Matriz[2][3]*solucoes[3])))+Matriz[2][12])/Matriz[2][2]);
        solucoes[1] = (int) (((-1*((Matriz[1][11]*solucoes[11])+(Matriz[1][10]*solucoes[10])+(Matriz[1][9]*solucoes[9])+(Matriz[1][8]*solucoes[8])+(Matriz[1][7]*solucoes[7])+(Matriz[1][6]*solucoes[6])+(Matriz[1][5]*solucoes[5])+(Matriz[1][4]*solucoes[4])+(Matriz[1][3]*solucoes[3])+(Matriz[1][2]*solucoes[2])))+Matriz[1][12])/Matriz[1][1]);
        solucoes[0] = (int) (((-1*((Matriz[0][11]*solucoes[11])+(Matriz[0][10]*solucoes[10])+(Matriz[0][9]*solucoes[9])+(Matriz[0][8]*solucoes[8])+(Matriz[0][7]*solucoes[7])+(Matriz[0][6]*solucoes[6])+(Matriz[0][5]*solucoes[5])+(Matriz[0][4]*solucoes[4])+(Matriz[0][3]*solucoes[3])+(Matriz[0][2]*solucoes[2])+(Matriz[0][1]*solucoes[1])))+Matriz[0][12])/Matriz[0][0]);

        System.out.println("\n\n\n\n");
        for (int i=0;i<12;i++)
            System.out.println(solucoes[i]+ "  ");
    }

    private void zeraValores() {
        for (int i=0;i<12;i++) {
            for (int j=0;j<13;j++) {
                if (Matriz[i][j]<0.000001)
                    Matriz[i][j]=0;
            }
        }
    }



}
